import socket
import struct
import math
import csv
import os

#其他文件可以通过导入 get_dcs_data 函数来调用本文件
#from getDCSdata import get_dcs_data
#get_dcs_data(udp_ip="127.0.0.1", udp_port=12345)


def get_dcs_data(udp_ip="127.0.0.1", udp_port=12345):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind((udp_ip, udp_port))

    # 修正后的预期数据长度（与C++的bufferSize一致）
    expected_length = 264  # 276 - 12 = 264
    print(f"[Python] Expected data length: {expected_length} bytes")

    # 定义 CSV 文件名
    csv_filename = "results/getDCSdata.csv"

    # 检查文件是否存在，如果不存在则创建并写入表头
    file_exists = os.path.exists(csv_filename)
    with open(csv_filename, 'a', newline='') as csvfile:
        writer = csv.writer(csvfile)
        if not file_exists:
            # 写入表头
            writer.writerow([
                "ID", "targetID", "gunID", "flags", "M_", "IAS", "heading_deg", "AoA",
                "Point_X", "Point_Y", "Point_Z", "VV_X", "VV_Y", "VV_Z", "VV_0",
                "ACC_X", "ACC_Y", "ACC_Z",
                "X_Axis_X", "X_Axis_Y", "X_Axis_Z",
                "Y_Axis_X", "Y_Axis_Y", "Y_Axis_Z",
                "Z_Axis_X", "Z_Axis_Y", "Z_Axis_Z",
                "Omega_X", "Omega_Y", "Omega_Z",
                "pitch_deg", "bank_deg", "yaw_deg",
                "LLA_long", "LLA_lat", "LLA_balt",
                "Gears", "Flaps", "AirBrake", "Fuel",
                "RPM_0", "RPM_1", "RPM_2", "RPM_3", "RPM_4", "RPM_5"
            ])

    while True:
        data, addr = sock.recvfrom(65536)
        if len(data) != expected_length:
            print(f"Invalid data length: {len(data)} bytes (expected {expected_length})")
            continue

        try:
            offset = 0

            # --- 解析字段（与C++顺序严格一致）---
            # 1. 基础信息（4个uint32_t）
            ID = struct.unpack_from('I', data, offset)[0]; offset += 4
            targetID = struct.unpack_from('I', data, offset)[0]; offset += 4
            gunID = struct.unpack_from('I', data, offset)[0]; offset += 4
            flags = struct.unpack_from('i', data, offset)[0]; offset += 4
            hex_flags = hex(flags) # 转为16进制

            # 2. 运动参数（4个float）
            M_ = struct.unpack_from('f', data, offset)[0]; offset += 4
            IAS = struct.unpack_from('f', data, offset)[0]; offset += 4  #指示空速
            heading = struct.unpack_from('f', data, offset)[0]; offset += 4
            AoA = struct.unpack_from('f', data, offset)[0]; offset += 4

            # 3. 位置、速度、加速度（各3个double）
            Point = struct.unpack_from('3d', data, offset); offset += 24
            VV = struct.unpack_from('3d', data, offset); offset += 24
            VV_0 = math.sqrt(VV[0]*VV[0] + VV[1]*VV[1] + VV[2]*VV[2])  # 对地速度  新增
            ACC = struct.unpack_from('3d', data, offset); offset += 24

            # 4. 三轴矢量（各3个double）
            X_Axis = struct.unpack_from('3d', data, offset); offset += 24
            Y_Axis = struct.unpack_from('3d', data, offset); offset += 24
            Z_Axis = struct.unpack_from('3d', data, offset); offset += 24

            # 5. 角速度（3个double）
            Omega = struct.unpack_from('3d', data, offset); offset += 24

            # 6. 姿态角（3个float）
            pitch = struct.unpack_from('f', data, offset)[0]; offset += 4
            bank = struct.unpack_from('f', data, offset)[0]; offset += 4
            yaw = struct.unpack_from('f', data, offset)[0]; offset += 4

            # 7. 经纬高（3个float，原错误解析为double）
            LLA_long = struct.unpack_from('f', data, offset)[0]; offset += 4
            LLA_lat = struct.unpack_from('f', data, offset)[0]; offset += 4
            LLA_balt = struct.unpack_from('f', data, offset)[0]; offset += 4
            # 确定经度和纬度的符号
            long_sign = 'E' if LLA_long > 0 else 'W'  # 经度
            lat_sign = 'N' if LLA_lat > 0 else 'S'  # 纬度

            # 8. 机械状态（4个float）
            Gears = struct.unpack_from('f', data, offset)[0]; offset += 4
            Flaps = struct.unpack_from('f', data, offset)[0]; offset += 4
            AirBrake = struct.unpack_from('f', data, offset)[0]; offset += 4
            Fuel = struct.unpack_from('f', data, offset)[0]; offset += 4

            # 9. RPM（6个float）
            RPM = []
            for _ in range(6):
                rpm_value = struct.unpack_from('f', data, offset)[0]
                RPM.append(rpm_value)
                offset += 4

            # --- 格式化RPM输出 ---
            # 前两个RPM值保留四位有效数字，其余保持原样
            formatted_RPM = [f"{rpm:.4g}" if i < 2 else f"{rpm:.4f}" for i, rpm in enumerate(RPM)]

            # --- 弧度转换为度 ---
            heading_deg = math.degrees(heading)  # 真航向
            pitch_deg = math.degrees(pitch)  # 俯仰角
            bank_deg = math.degrees(bank)  # 滚转角
            yaw_deg = math.degrees(yaw)  # 偏航角

            # # --- 输出全部信息 ---
            # print("\n=== 接收到的全部信息 ===")
            # print(f"ID: {ID}")
            # print(f"目标ID: {targetID}")
            # print(f"发射平台ID: {gunID}")
            # print(f"状态标识: {hex_flags}")
            # print(f"马赫数: {M_:.3f} M")
            # print(f"指示空速: {IAS:.2f} m/s")
            # print(f"对地速度：{VV_0:.2f} m/s")  # 新增  飞行控制或性能优化：使用空速作为参数。路径规划或导航任务：使用地速作为参数。
            # print(f"航向: {heading_deg:.2f} deg")
            # print(f"攻角: {AoA:.2f} deg")
            # print(f"位置 (北天东): ({Point[0]:.3f}, {Point[1]:.3f}, {Point[2]:.3f}) m")
            # print(f"速度 (北天东): ({VV[0]:.2f}, {VV[1]:.2f}, {VV[2]:.2f}) m/s")
            # print(f"加速度 (北天东): ({ACC[0]:.4f}, {ACC[1]:.4f}, {ACC[2]:.4f}) G")
            # print(f"X轴矢量: ({X_Axis[0]:.4f}, {X_Axis[1]:.4f}, {X_Axis[2]:.4f})")
            # print(f"Y轴矢量: ({Y_Axis[0]:.4f}, {Y_Axis[1]:.4f}, {Y_Axis[2]:.4f})")
            # print(f"Z轴矢量: ({Z_Axis[0]:.4f}, {Z_Axis[1]:.4f}, {Z_Axis[2]:.4f})")
            # print(f"角速度 (X/Y/Z): ({Omega[0]:.4f}, {Omega[1]:.4f}, {Omega[2]:.4f}) deg/s")
            # print(f"姿态角 (俯仰/滚转/偏航): ({pitch_deg:.4f}, {bank_deg:.4f}, {yaw_deg:.4f}) deg")
            # print(f"经纬高: (经度={long_sign} {LLA_long:.6f}, 纬度= {lat_sign} {LLA_lat:.6f}, 高度={LLA_balt:.2f} m)")
            # print(f"机械状态: 机轮={Gears:.2f}, 襟翼={Flaps:.2f}, 减速板={AirBrake:.2f}, 燃油={Fuel * 100:.2f}%")
            # print(f"引擎RPM: {RPM}")


            # print(f"引擎RPM: {', '.join(formatted_RPM)}")

            # --- 将数据写入 CSV 文件 ---
            with open(csv_filename, 'a', newline='') as csvfile:
                writer = csv.writer(csvfile)
                writer.writerow([
                    ID, targetID, gunID, hex_flags, M_, IAS, heading_deg, AoA,
                    Point[0], Point[1], Point[2], VV[0], VV[1], VV[2], VV_0,
                    ACC[0], ACC[1], ACC[2],
                    X_Axis[0], X_Axis[1], X_Axis[2],
                    Y_Axis[0], Y_Axis[1], Y_Axis[2],
                    Z_Axis[0], Z_Axis[1], Z_Axis[2],
                    Omega[0], Omega[1], Omega[2],
                    pitch_deg, bank_deg, yaw_deg,
                    LLA_long, LLA_lat, LLA_balt,
                    Gears, Flaps, AirBrake, Fuel,
                    RPM[0], RPM[1], RPM[2], RPM[3], RPM[4], RPM[5]
                ])

            # 返回解析后的数据
            return {
                "ID": ID,
                "targetID": targetID,
                "gunID": gunID,
                "flags": hex_flags,
                "M_": M_,
                "IAS": IAS,
                "heading_deg": heading_deg,
                "AoA": AoA,
                "Point": Point,
                "VV": VV,
                "VV_0": VV_0,
                "ACC": ACC,
                "X_Axis": X_Axis,
                "Y_Axis": Y_Axis,
                "Z_Axis": Z_Axis,
                "Omega": Omega,
                "pitch_deg": pitch_deg,
                "bank_deg": bank_deg,
                "yaw_deg": yaw_deg,
                "LLA_long": LLA_long,
                "LLA_lat": LLA_lat,
                "LLA_balt": LLA_balt,
                "Gears": Gears,
                "Flaps": Flaps,
                "AirBrake": AirBrake,
                "Fuel": Fuel,
                "RPM": RPM
            }


        except struct.error as e:
            print(f"Parse error: {e}")

# 如果直接运行此脚本，则启动数据接收
if __name__ == "__main__":
    get_dcs_data()